%Main Particle Filter method.
% initialLocation is [x, y, theta]
% particle_simulated(M,T,[5 5])
function [finalLocation,iterations] =  run_simulator (map, target, initialLocation)
simulatedRobot = initialLocation;
particles = initialise(map);
plan = [0, 0];
req_weight = 0.95; %The required weighting of the largest particle to check for termination
numReadings = 9; %The number of readings in a sweep
readingRange = pi; %The angle range of a sweep
padmap = mapPad(map, 1);
roundingAccuracy = 3;
roundingFactor = 1/roundingAccuracy;
maxIterations = 40; %The maximum number of iterations before terminating

%Create base array for angles of read
if numReadings == 1
    readingAnglesBase = 0;
else
    readingAnglesBase = (0:numReadings - 1)';
    interval = readingRange/(numReadings - 1);
    readingAnglesBase = readingAnglesBase .* interval;
    readingAnglesBase = readingAnglesBase - (readingRange/2);
end
iterations = 0;
%Main loop
while iterations < maxIterations
    iterations = iterations + 1;
    simulatedRobot = simulator_rotate(simulatedRobot, plan(2));
    interruptReading = getSimulatedReading(simulatedRobot, map);
    [simulatedRobot, plan(1)] = simulator_move(simulatedRobot, interruptReading, plan(1)); %already contains code to stop robot leaving map
    
    particles = move_particles(particles, plan);
    
    %displaySimulatedRobot(simulatedRobot, map);
    displaySimulatedRobotAndParticles(simulatedRobot, particles, map);
    
    angles = readingAnglesBase + simulatedRobot(3);
    signal = getSimulatedReadingSweep(simulatedRobot, map, angles);
    %signal = getSimulatedReading(simulatedRobot, map);
    
    particle_signals = [];
    for i = 1:numReadings
        particle_signals = [particle_signals, get_particle_measurements(particles, map, readingAnglesBase(i))];
    end
    %Reweight returns a sorted list of particles, according to weights, biggest first.
    particles = reweight(particles, particle_signals, signal, map);
    
    disp('Actual location: ')
    disp(simulatedRobot)
    disp('Current best guess: ')
    disp(particles(1, :))
    
    %If the robot is lost reinitialise the map, and then continue
    %looping.
    [valid,center] = found(particles);
    if is_lost(particles(1, 4));
        disp('lost')
        particles = initialise(map);
        plan = [0 0];
        %plan = plan_motion(particles(1,1:2),target, map, particles(1,3));
        %         plan = [10*rand(1), pi/2*rand(1) - pi/4];
        %If the biggest particle's weight is greater than the threshhold,
        %and the robot is at the target point, stop looping.
    elseif (valid == 1) & (round(roundingFactor*center(1))/roundingFactor == round(roundingFactor*target(1))/roundingFactor) & (round(roundingFactor*center(2))/roundingFactor == round(roundingFactor*target(2))/roundingFactor)
        %             particles(1, 2) > req_weight && (particles(1, 1) == target(1)) && (particles(1, 2) == target(2))
        
        angles = readingAnglesBase + particles(1, 3);
        signal = getSimulatedReadingSweep(simulatedRobot, map, angles);
        particle_signals = [];
        for i = 1:length(angles)
            particle_signals = [particle_signals, get_particle_measurements(particles, map, readingAnglesBase(i))];
        end
        particles = reweight(particles, particle_signals, signal, map);
        [valid,center] = found(particles)
        if (valid == 1) & (round(roundingFactor*center(1))/roundingFactor == round(roundingFactor*target(1))/roundingFactor) & (round(roundingFactor*center(2))/roundingFactor == round(roundingFactor*target(2))/roundingFactor)
            finalLocation = particles(1, 1:2);
            displaySimulatedRobotAndParticles(particles(1,:), particles, map);
            break
        end
        for i = 1:length(particles)
            if inpolygon(particles(i, 1), particles(i, 2), padmap(:,1), padmap(:, 2))
                plan = plan_motion(particles(i,1:2),target, padmap, particles(i,3));
                break
            end
        end
    else
        %plan = plan_motion(particles(1, 1), target);
        for i = 1:length(particles)
            if inpolygon(particles(i, 1), particles(i, 2), padmap(:,1), padmap(:, 2))
                plan = plan_motion(particles(i,1:2),target, padmap, particles(i,3));
                break
            end
        end
        
        particles = resample(particles, map);
        %         plan = [rand(1), pi/2*rand(1) - pi/4]; %random movement for testing
        %particles = normalise(particles); Why is this commented out?
    end
    if size(particles, 1) == 0
        particles = initialise(map);
    end
    if iterations == maxIterations
        finalLocation = simulatedRobot(1:2);
        disp('Reached maximum iterations');
    end
end